
import roslibpy
import json


class SportModeState:
    mode: int
    gait_type: int
    foot_raise_height: float
    position: list[float]
    body_height: float
    velocity: list[float]
    yaw_speed: float
    range_obstacle: list[float]

    """
    运动模式状态
    """
    def __init__(self,msg_dict:dict) -> None:
        self.mode = msg_dict.get('mode',0)
        self.gait_type = msg_dict.get('gait_type',0)
        self.foot_raise_height = msg_dict.get('foot_raise_height',0.0)
        self.position = msg_dict.get('position',[0.0,0.0,0.0])
        self.body_height = msg_dict.get('body_height',0.0)
        self.velocity = msg_dict.get('velocity',[0.0,0.0,0.0])
        self.yaw_speed = msg_dict.get('yaw_speed',0.0)
        self.range_obstacle = msg_dict.get('range_obstacle',[0.0,0.0,0.0,0.0])

"""

TimeSpec stamp //时间戳
uint32 error_code //错误代码
IMUState imu_state //IMU状态
uint8 mode //运动模式
/*
运动模式
0. idle, default stand
1. balanceStand
2. pose
3. locomotion
4. reserve
5. lieDown
6. jointLock
7. damping
8. recoveryStand
9. reserve
10. sit
11. frontFlip
12. frontJump
13. frontPounc
*/
float32 progress //是否动作执行状态：0. dance false; 1. dance true
uint8 gait_type //步态类型
/*
步态类型
0.idle  
1.trot  
2.run  
3.climb stair  
4.forwardDownStair   
9.adjust
*/
float32 foot_raise_height //抬腿高度
float32[3] position //当前位置
float32 body_height //机体高度
float32[3] velocity //线速度
float32 yaw_speed //偏行速度
float32[4] range_obstacle //障碍物范围 
int16[4] foot_force //足端力数值
float32[12] foot_position_body //足端相对于机体的位置
float32[12] foot_speed_body //足端相对于机体的速度

"""